#include "SceneDrawer.h"
#include "GL/glut.h"

/*static member initialization*/
XnBool SceneDrawer::g_bPause = false;
XnBool SceneDrawer::g_bRecord = false; 
XnBool SceneDrawer::g_bQuit = false;
XnBool SceneDrawer::g_bNeedPose = FALSE;
XnChar SceneDrawer::g_strPose[20] = "";
XnBool SceneDrawer::g_bDrawBackground = TRUE;
XnBool SceneDrawer::g_bDrawPixels = TRUE;
XnBool SceneDrawer::g_bDrawSkeleton = TRUE;
XnBool SceneDrawer::g_bPrintID = TRUE;
XnBool SceneDrawer::g_bPrintState = TRUE;
XnFloat SceneDrawer::Colors[][3] =
	{
		{0,1,1},
		{0,0,1},
		{0,1,0},
		{1,1,0},
		{1,0,0},
		{1,.5,0},
		{.5,1,0},
		{0,.5,1},
		{.5,0,1},
		{1,1,.5},
		{1,1,1}
	};
XnUInt32 SceneDrawer::nColors = 10;
float SceneDrawer::g_pDepthHist[MAX_DEPTH];
float SceneDrawer::texcoords[8];
KinectMsg SceneDrawer::msg;
bool SceneDrawer::bNewMsg = false;

SceneDrawer::SceneDrawer(void)
{

}


SceneDrawer::~SceneDrawer(void)
{

}


void SceneDrawer::glInit(int *argc, char ** argv)
{
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
	glutCreateWindow ("Prime Sense User Tracker Viewer");
	//glutFullScreen();
	glutSetCursor(GLUT_CURSOR_NONE);

	glutKeyboardFunc(glutKeyboard);
	glutDisplayFunc(glutDisplay);
	glutIdleFunc(glutIdle);

	glDisable(GL_DEPTH_TEST);
	glEnable(GL_TEXTURE_2D);

	glEnableClientState(GL_VERTEX_ARRAY);
	glDisableClientState(GL_COLOR_ARRAY);
}

int SceneDrawer::glMain(int argc, char **argv) {
	glInit(&argc, argv);
	glutMainLoop();
	return 0;
}

void SceneDrawer::glutKeyboard (unsigned char key, int x, int y)
{
	switch (key)
	{
	case 27:
		CleanupExit();
	case 'b':
		// Draw background?
		g_bDrawBackground = !g_bDrawBackground;
		break;
	case 'x':
		// Draw pixels at all?
		g_bDrawPixels = !g_bDrawPixels;
		break;
	case 's':
		// Draw Skeleton?
		g_bDrawSkeleton = !g_bDrawSkeleton;
		break;
	case 'i':
		// Print label?
		g_bPrintID = !g_bPrintID;
		break;
	case 'l':
		// Print ID & state as label, or only ID?
		g_bPrintState = !g_bPrintState;
		break;
	case'p':
		g_bPause = !g_bPause;
		break;
	}
}

void SceneDrawer::CleanupExit() {

	exit(1);
}

void SceneDrawer::glutDisplay (void) {
	glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	// Setup the OpenGL viewpoint
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();

	glOrtho(0, msg.XRes, msg.YRes, 0, -1.0, 1.0);

	glDisable(GL_TEXTURE_2D);
	DrawDepthMap();

	glutSwapBuffers();

}

void SceneDrawer::glutIdle (void) {
	if (g_bQuit) {
		CleanupExit();
	}

	// Display the frame
	glutPostRedisplay();
}

unsigned int SceneDrawer::getClosestPowerOfTwo(unsigned int n)
{
	unsigned int m = 2;
	while(m < n) m<<=1;

	return m;
}

unsigned int SceneDrawer::initTexture(void** buf, int& width, int& height)
{
	GLuint texID = 0;
	glGenTextures(1,&texID);

	width = getClosestPowerOfTwo(width);
	height = getClosestPowerOfTwo(height); 
	*buf = new unsigned char[width*height*4];
	glBindTexture(GL_TEXTURE_2D,texID);

	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);

	return texID;
}


void SceneDrawer::DrawRectangle(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	float verts[8] = {	topLeftX, topLeftY,
		topLeftX, bottomRightY,
		bottomRightX, bottomRightY,
		bottomRightX, topLeftY
	};
	glVertexPointer(2, GL_FLOAT, 0, verts);
	glDrawArrays(GL_TRIANGLE_FAN, 0, 4);

	//TODO: Maybe glFinish needed here instead - if there's some bad graphics crap
	glFlush();
}
void SceneDrawer::DrawTexture(float topLeftX, float topLeftY, float bottomRightX, float bottomRightY)
{
	glEnableClientState(GL_TEXTURE_COORD_ARRAY);
	glTexCoordPointer(2, GL_FLOAT, 0, texcoords);

	DrawRectangle(topLeftX, topLeftY, bottomRightX, bottomRightY);

	glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}

void SceneDrawer::glPrintString(void *font, char *str)
{
	int i,l = strlen(str);

	for(i=0; i<l; i++)
	{
		glutBitmapCharacter(font,*str++);
	}
}



void SceneDrawer::DrawDepthMap()
{
	static bool bInitialized = false;	
	static GLuint depthTexID;
	static unsigned char* pDepthTexBuf;
	static int texWidth, texHeight;

	float topLeftX;
	float topLeftY;
	float bottomRightY;
	float bottomRightX;
	float texXpos;
	float texYpos;

	if(!bInitialized)
	{

		texWidth =  getClosestPowerOfTwo(msg.XRes);
		texHeight = getClosestPowerOfTwo(msg.YRes);

		//		printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
		depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;

		//		printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
		bInitialized = true;

		topLeftX = msg.XRes;
		topLeftY = 0;
		bottomRightY = msg.YRes;
		bottomRightX = 0;
		texXpos =(float)msg.XRes/texWidth;
		texYpos  =(float)msg.YRes/texHeight;

		memset(texcoords, 0, 8*sizeof(float));
		texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;

	}
	unsigned int nValue = 0;
	unsigned int nHistValue = 0;
	unsigned int nIndex = 0;
	unsigned int nX = 0;
	unsigned int nY = 0;
	unsigned int nNumberOfPoints = 0;
	XnUInt16 g_nXRes = msg.XRes;
	XnUInt16 g_nYRes = msg.YRes;

	unsigned char* pDestImage = pDepthTexBuf;

	const XnDepthPixel* pDepth = msg.depthMD;
	const XnLabel* pLabels = msg.sceneMD;

	// Calculate the accumulative histogram
	memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
	for (nY=0; nY<g_nYRes; nY++)
	{
		for (nX=0; nX<g_nXRes; nX++)
		{
			nValue = *pDepth;

			if (nValue != 0)
			{
				g_pDepthHist[nValue]++;
				nNumberOfPoints++;
			}

			pDepth++;
		}
	}

	for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
	{
		g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
	}
	if (nNumberOfPoints)
	{
		for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
		{
			g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
		}
	}

	pDepth = msg.depthMD;
	if (g_bDrawPixels)
	{
		XnUInt32 nIndex = 0;
		// Prepare the texture map
		for (nY=0; nY<g_nYRes; nY++)
		{
			for (nX=0; nX < g_nXRes; nX++, nIndex++)
			{

				pDestImage[0] = 0;
				pDestImage[1] = 0;
				pDestImage[2] = 0;
				if (g_bDrawBackground || *pLabels != 0)
				{
					nValue = *pDepth;
					XnLabel label = *pLabels;
					XnUInt32 nColorID = label % nColors;
					if (label == 0)
					{
						nColorID = nColors;
					}

					if (nValue != 0)
					{
						nHistValue = g_pDepthHist[nValue];

						pDestImage[0] = nHistValue * Colors[nColorID][0]; 
						pDestImage[1] = nHistValue * Colors[nColorID][1];
						pDestImage[2] = nHistValue * Colors[nColorID][2];
					}
				}

				pDepth++;
				pLabels++;
				pDestImage+=3;
			}

			pDestImage += (texWidth - g_nXRes) *3;
		}
	}
	else
	{
		xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
	}

	glBindTexture(GL_TEXTURE_2D, depthTexID);
	glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);

	// Display the OpenGL texture map
	glColor4f(0.75,0.75,0.75,1);

	glEnable(GL_TEXTURE_2D);
	DrawTexture(msg.XRes,msg.YRes,0,0);	
	glDisable(GL_TEXTURE_2D);

	if(msg.body.bTracking) {
		glBegin(GL_LINES);
		glColor4f(1-Colors[1%nColors][0], 1-Colors[1%nColors][1], 1-Colors[1%nColors][2], 1);
		if(msg.body.bHead && msg.body.bNeck) {
			glVertex3i(msg.body.pHead.X, msg.body.pHead.Y, 0);
			glVertex3i(msg.body.pNeck.X, msg.body.pNeck.Y, 0);
		}

		if(msg.body.bLShoulder && msg.body.bNeck) {
			glVertex3i(msg.body.pLShoulder.X, msg.body.pLShoulder.Y, 0);
			glVertex3i(msg.body.pNeck.X, msg.body.pNeck.Y, 0);
		}
		if(msg.body.bLShoulder && msg.body.bLElbow) {
			glVertex3i(msg.body.pLShoulder.X, msg.body.pLShoulder.Y, 0);
			glVertex3i(msg.body.pLElbow.X, msg.body.pLElbow.Y, 0);
		}
		if(msg.body.bLHand && msg.body.bLElbow) {
			glVertex3i(msg.body.pLHand.X, msg.body.pLHand.Y, 0);
			glVertex3i(msg.body.pLElbow.X, msg.body.pLElbow.Y, 0);
		}

		if(msg.body.bRShoulder && msg.body.bNeck) {
			glVertex3i(msg.body.pRShoulder.X, msg.body.pRShoulder.Y, 0);
			glVertex3i(msg.body.pNeck.X, msg.body.pNeck.Y, 0);
		}
		if(msg.body.bRShoulder && msg.body.bRElbow) {
			glVertex3i(msg.body.pRShoulder.X, msg.body.pRShoulder.Y, 0);
			glVertex3i(msg.body.pRElbow.X, msg.body.pRElbow.Y, 0);
		}
		if(msg.body.bRHand && msg.body.bRElbow) {
			glVertex3i(msg.body.pRHand.X, msg.body.pRHand.Y, 0);
			glVertex3i(msg.body.pRElbow.X, msg.body.pRElbow.Y, 0);
		}

		if(msg.body.bLShoulder && msg.body.bTorso) {
			glVertex3i(msg.body.pLShoulder.X, msg.body.pLShoulder.Y, 0);
			glVertex3i(msg.body.pTorso.X, msg.body.pTorso.Y, 0);
		}
		if(msg.body.bRShoulder && msg.body.bTorso) {
			glVertex3i(msg.body.pRShoulder.X, msg.body.pRShoulder.Y, 0);
			glVertex3i(msg.body.pTorso.X, msg.body.pTorso.Y, 0);
		}

		if(msg.body.bLHip && msg.body.bTorso) {
			glVertex3i(msg.body.pLHip.X, msg.body.pLHip.Y, 0);
			glVertex3i(msg.body.pTorso.X, msg.body.pTorso.Y, 0);
		}
		if(msg.body.bLHip && msg.body.bLKnee) {
			glVertex3i(msg.body.pLHip.X, msg.body.pLHip.Y, 0);
			glVertex3i(msg.body.pLKnee.X, msg.body.pLKnee.Y, 0);
		}
		if(msg.body.bLFoot && msg.body.bLKnee) {
			glVertex3i(msg.body.pLFoot.X, msg.body.pLFoot.Y, 0);
			glVertex3i(msg.body.pLKnee.X, msg.body.pLKnee.Y, 0);
		}

		if(msg.body.bRHip && msg.body.bTorso) {
			glVertex3i(msg.body.pRHip.X, msg.body.pRHip.Y, 0);
			glVertex3i(msg.body.pTorso.X, msg.body.pTorso.Y, 0);
		}
		if(msg.body.bRHip && msg.body.bRKnee) {
			glVertex3i(msg.body.pRHip.X, msg.body.pRHip.Y, 0);
			glVertex3i(msg.body.pRKnee.X, msg.body.pRKnee.Y, 0);
		}
		if(msg.body.bRFoot && msg.body.bRKnee) {
			glVertex3i(msg.body.pRFoot.X, msg.body.pRFoot.Y, 0);
			glVertex3i(msg.body.pRKnee.X, msg.body.pRKnee.Y, 0);
		}

		if(msg.body.bRHip && msg.body.bLHip) {
			glVertex3i(msg.body.pRHip.X, msg.body.pRHip.Y, 0);
			glVertex3i(msg.body.pLHip.X, msg.body.pLHip.Y, 0);
		}

		glEnd();
	}
	

}